#include "opticalFlow.h"

opticalFlow::opticalFlow (int window, int displacement, int wSize, int hSize)
	{
		m_window= window;
		m_displacement = displacement;
		m_wSize = wSize;
		m_hSize = hSize;

		//for (unsigned int i=0; i< (hSize - 2*(window+displacement)) * (wSize - 2*(window+displacement)); i+=1)
		for ( int i=0; i< hSize*wSize ; i+=1)
			m_velocities.push_back (vec2 ());

		Ix = new float [ wSize*hSize];
		Iy = new float[ wSize*hSize];
		It = new float [ wSize*hSize];
		gaussOldImage = new unsigned char [ wSize*hSize];
		gaussNewImage =new unsigned char [ wSize*hSize];
		for ( int i=0; i< hSize*wSize ; i+=1)
		{
			gaussOldImage[i]=0;
			gaussNewImage[i]=0;
		}

		
	}

	void opticalFlow::calculateColor (vec3 *color, vec2 * vel)
	{
		float rad = atan (vel->getY()/vel->getX());
		float grad = rad*180.0f/3.14f;
		if (vel->getY()>0)
			if (vel->getX()<0)
				grad = 180-grad;
		if (vel->getY()<0)
			if (vel->getX()<0)
				grad = 180+grad;
			else
				grad = 360-grad;

		float h, s, v; 		   
	h = grad;
	
	
   
   s = 0.65f;
   v = 0.95f;


//algoritmo que transforma HSV em RGB
 	float  f, p, q, t;
	int hi;
	
	h /= 60;			// sector 0 to 5
	hi = (int) floor( h );
	f = h - hi;			// factorial part of h
	p = v * ( 1 - s );
	q = v * ( 1 - s * f );
	t = v * ( 1 - s * ( 1 - f ) );

    switch ( hi ) 
    { 
	case 0  : color->setPoints (v,t,p); break; 
    case 1  : color->setPoints (q,v,p); break; 
    case 2  : color->setPoints (p,v,t); break; 
    case 3  : color->setPoints (p,q,v); break; 
    case 4  : color->setPoints (t,p,v); break; 
    case 5  : color->setPoints (v,p,q); break;    
	 };


	}

	void opticalFlow::calculateIxIyIt (unsigned char* oldScreen, unsigned char* newScreen)
	{
		for ( int i=1; i< m_wSize-1 ; i+=1)
			for ( int j=1; j< m_hSize-1 ; j+=1)
			{
				Ix [i+j*m_wSize] = (oldScreen[ (i+1)+j*m_wSize]  - oldScreen[ (i-1)+j*m_wSize] + newScreen[ (i+1)+j*m_wSize]  - newScreen[ (i-1)+j*m_wSize])*0.5f;
				Iy [i+j*m_wSize] = (oldScreen[ i+(j+1)*m_wSize]  - oldScreen[ i+(j-1)*m_wSize] + newScreen[ i+(j+1)*m_wSize]  - newScreen[ i+(j+1)*m_wSize])*0.5f;
				It [i+j*m_wSize] = newScreen[ i+j*m_wSize]  - oldScreen[ i+j*m_wSize];
			}



	}

	void opticalFlow::calculateUV (int pixelX, int pixelY)
	{
		float u,v;
		u = matrixA[0][0]*vectorB[0]+matrixA[0][1]*vectorB[1];
		v = matrixA[1][0]*vectorB[0]+matrixA[1][1]*vectorB[1];

		m_velocities[pixelX + pixelY*m_wSize].setPoints (u,v);

	}

	void opticalFlow::calculateAB (int pixelX, int pixelY, int neighborSize)
	{
		matrixA[0][0]=0;
		matrixA[0][1]=0;
	//	matrixA[1][0]=0;
		matrixA[1][1]=0;
		vectorB[0]=0;
		vectorB[1]=0;
		for (int x= pixelX-1; x<= pixelX+1; x+=1)
			for (int y= pixelY-1; y<= pixelY+1; y+=1)
			{
				matrixA[0][0]+= Ix[x + y*m_wSize]*Ix[x + y*m_wSize];
				matrixA[0][1]+= Ix[x + y*m_wSize]*Iy[x + y*m_wSize];
				matrixA[1][1]+= Iy[x + y*m_wSize]*Iy[x + y*m_wSize];
				vectorB[0]+=(int)(Ix[x + y*m_wSize]*It[x + y*m_wSize]);
				vectorB[1]+=(int)(Iy[x + y*m_wSize]*It[x + y*m_wSize]);

			}
		matrixA[1][0]=matrixA[0][1];


	}

	void opticalFlow::inverseA ()
	{
		float oneByDet = 1.0f/ (matrixA[0][0]*matrixA[1][1]-matrixA[0][1]*matrixA[1][0]); //eh 1/det
		//printf ("\n1/Det %f",oneByDet);
		float aux =  matrixA[0][0];
		matrixA[0][0]= matrixA[1][1];
		matrixA[1][1]= aux;
		matrixA[0][1]=-matrixA[0][1];
		matrixA[1][0]=-matrixA[1][0];
		matrixA[0][0]*= oneByDet;
		matrixA[1][1]*= oneByDet;
		matrixA[0][1]*=oneByDet;
		matrixA[1][0]*=oneByDet;
	}

	void opticalFlow::calculate (unsigned char* oldScreen, unsigned char* newScreen)
	{
		//printf ("\nOptical Flow 1"); //DEBUG
		Convolution c;
		float matrixGauss[9] = { 1.0f/9.0f,1.0f/9.0f,1.0f/9.0f,1.0f/9.0f,1.0f/9.0f,1.0f/9.0f,1.0f/9.0f,1.0f/9.0f,1.0f/9.0f};
		
		c.applyConvolution (oldScreen,256,256,matrixGauss,3,gaussOldImage);
		c.applyConvolution (newScreen,256,256,matrixGauss,3,gaussNewImage);
		//printf ("\nOptical Flow 2"); //DEBUG

		calculateIxIyIt (gaussOldImage, gaussNewImage);
		//printf ("\nOptical Flow 3"); //DEBUG
		int neighborSize=1;
		for (int x = neighborSize; x< m_wSize - neighborSize; x+=1)
			for (int y = neighborSize; y< m_hSize - neighborSize; y+=1)
			{
				calculateAB (x,y,neighborSize);
				inverseA ();
				calculateUV (x,y);
			}
		//printf ("\nOptical Flow 8"); //DEBUG


	/*	for(int x1 = (m_window + m_displacement + 1); x1 < (m_wSize - m_window - m_displacement); x1+=1) 
			for(int y1 = (m_window + m_displacement + 1); y1 < (m_hSize - m_window - m_displacement); y1+=1) 
			{
				int min = INT_MAX;
				int dx=0;
				int dy=0;
				// loop through the displacement area
				for(int x2 = (x1 - m_displacement); x2 < (x1 + m_displacement); x2+=1) 
					for(int y2 = (y1 - m_displacement); y2 < (y1 + m_displacement); y2+=1) 
					{
						int sum = 0;
						for(int i = -m_window; i < m_window; i+=1) 
							for(int j = -m_window; j < m_window; j+=1) 
							// sum pixel intensity difference squared over the window area
								sum += (oldScreen[(x1 + i) + (y1 + j) * m_wSize] - newScreen[(x2 + i) + (y2 + j) * m_wSize])*(oldScreen[(x1 + i) + (y1 + j) * m_wSize] - newScreen[(x2 + i) + (y2 + j) * m_wSize]);
						if(sum < min) {
								min = sum;
								dx = x2 - x1;
								dy = y2 - y1;
							}

					}
					m_velocities[x1 + y1 * m_wSize].setPoints (dx,dy);
					//printf ("Velocidade %d %d",dx,dy);
							
			}*/
		
	}

	void opticalFlow::draw (int xInit,int yInit,int xSize,int ySize)
	{
		float xStep, yStep;
		//xStep = (float)xSize/(float)m_wSize;//
		//yStep = (float)ySize/(float)m_hSize;//
		xStep = (float)ySize/(float)m_hSize;//
		yStep = (float)xSize/(float)m_wSize;//
		
		glDisable (GL_LIGHTING);
		glColor3f (0,1,0);
		glLineWidth (2);
		glBegin (GL_LINES);
		vec3 color;
		for(int x1 = 0; x1 < m_wSize; x1+=12) 
			for(int y1 = 0; y1 < m_hSize ; y1+=1) 
			{
				
				float velocityLength = m_velocities[x1 + y1 * m_wSize].getX()*m_velocities[x1 + y1 * m_wSize].getX()+m_velocities[x1 + y1 * m_wSize].getY()*m_velocities[x1 + y1 * m_wSize].getY();
				if (velocityLength>0)
				//printf ("\nvecLength %f",velocityLength);
				velocityLength = sqrt (velocityLength);

				if ( (velocityLength >11) )
				{

					/*if(m_velocities[x1 + y1 *m_wSize].getY()<0)
						glColor3f (0,1,0);
					else
						glColor3f (0,0,1);*/
					
					if(m_velocities[x1 + y1 *m_wSize].getX()<0)
						glColor3f (0,1,0);
					else
						glColor3f (0,0,1);
						
					//calculateColor ( &color,&m_velocities[x1 + y1 *m_wSize]);
					//glColor4fv (color.getData ());
				glVertex3f (xInit+y1*xStep,(yInit+ySize)-x1*yStep,1);
				//glVertex3f (xInit+y1*xStep+0.2f*m_velocities[x1 + y1 *m_wSize].getY(),(yInit+ySize)-x1*yStep+0.2f*m_velocities[x1 + y1 * m_wSize].getX(),1);
				glVertex3f (xInit+y1*xStep,(yInit+ySize)-x1*yStep+0.2f*m_velocities[x1 + y1 * m_wSize].getX(),1);
				//glVertex3f (xInit+y1*xStep+0.01f*m_velocities[x1 + y1 *m_wSize].getY(),(yInit+ySize)-x1*yStep+0.01f*m_velocities[x1 + y1 * m_wSize].getX(),1);
				//glVertex3f (xInit+y1*xStep+m_velocities[x1 + y1 *m_wSize].getY(),(yInit+ySize)-x1*yStep,1);
				//glVertex3f (xInit+y1*xStep,(yInit+ySize)-x1*yStep+m_velocities[x1 + y1 * m_wSize].getX(),1);

				}
			}
			glEnd ();
			glColor3f (1,0,0);
			glPointSize(1);
		glBegin (GL_POINTS);
			for(int x1 = 0; x1 < m_wSize; x1+=12) 
			for(int y1 = 0; y1 < m_hSize ; y1+=1) 
				glVertex3f (xInit+y1*xStep,(yInit+ySize)-x1*yStep,1);
				
			
		glEnd ();
		glEnable(GL_LIGHTING);
		glLineWidth (1);
		/*planeFilter[0].setPoints (50,50,0);
		planeFilter[1].setPoints (150,50,0);
		planeFilter[2].setPoints (150,-50,0);
		planeFilter[3].setPoints (50,-50,0);
		*/
	}
